﻿using UnityEngine;

namespace HotPlayer.Physic2D
{
    public static class HotGJK
    {
        public static bool TestCollide(Polygon2D shape1, Polygon2D shape2)
        {
            var direction = new Vector2(1, 0);
            var support = GJKUtils.Support(shape1, shape2, direction);
            var simplex = GJKUtils.cleanSimplex;
            simplex.Add(support);
            direction = -support;
            var loopHunter = 0;
            while (loopHunter++ < 1024)
            {
                support = GJKUtils.Support(shape1, shape2, direction);
                if (Vector2.Dot(support, direction) < 0) return false;
                simplex.Add(support);

                if (NearestSimplex(simplex, ref direction)) return true;
            }

            return false;
        }

        public static bool NearestSimplex(Simplex simplex, ref Vector2 d)
        {
            if (simplex.Count == 2)
            {
                var a = simplex[0];
                var b = simplex[1];
                var newD = PerpendicularTo(a, b, Vector2.zero);
                d.x = newD.x;
                d.y = newD.y;
            }
            else if (simplex.Count == 3)
            {
                if (simplex.ContainsOrigin())
                {
                    return true;
                }

                //保留距离原点最近的边上的两个点，以这两个点的直线的垂线朝向原点的方向，作为下一次迭代方向
                var a = simplex[0];
                var b = simplex[1];
                var c = simplex[2];
                var dab = MathUtils.Distance(a, b, Vector2.zero); 
                var dac = MathUtils.Distance(a, c, Vector2.zero); 
                var dbc = MathUtils.Distance(b, c, Vector2.zero); 
                if (dbc < dac && dbc < dab)
                {
                    //remove a
                    simplex.RemoveAt(0);
                }
                else if (dac < dbc && dac < dab)
                {
                    //remove b
                    simplex.RemoveAt(1);
                }
                else
                {
                    //remove c
                    simplex.RemoveAt(2);
                }
                var newD = PerpendicularTo(simplex[0], simplex[1], Vector2.zero);
                d.x = newD.x;
                d.y = newD.y;
            }

            return false;
        }

        public static Vector2 PerpendicularTo(Vector2 a, Vector2 b, Vector2 p)
        {
            var direction = Vector2.Perpendicular(a - b);
            if (Vector2.Dot(direction, p - a) > 0)
            {
                return direction;
            }

            return -direction;
        }
    }
}